#ifndef PCL_WORLD_MODEL_H_
#define PCL_WORLD_MODEL_H_


#include <pcl/octree/octree.h>
#include <pcl/octree/octree_impl.h>

#include <pcl/filters/extract_indices.h>
#include <pcl/filters/filter_indices.h>

#include <pcl/filters/crop_box.h>

#include <pcl/filters/conditional_removal.h>

#include <pcl/point_types.h>

#include <pcl/io/pcd_io.h>

namespace pcl
{
  /** \brief WorldModel maintains a 3D point cloud that can be updated.
    * \The world is represented as a 3D grid which resolution can be set.
    * \When new points are added to the world, we make sure that each cell of the grid contains the latest values.
    * \author Raphael Favier
    */
  template <typename PointT>
  class WorldModel
  {
    public:

      typedef boost::shared_ptr<WorldModel<PointT> > Ptr;
      typedef boost::shared_ptr<const WorldModel<PointT> > ConstPtr;

      typedef pcl::PointCloud<PointT> PointCloud;
      typedef typename PointCloud::Ptr PointCloudPtr;
      typedef typename PointCloud::ConstPtr PointCloudConstPtr;

      typedef typename pcl::ConditionAnd<PointT>::Ptr ConditionAndPtr;
      typedef typename pcl::ConditionOr<PointT>::Ptr ConditionOrPtr;
      typedef typename pcl::FieldComparison<PointT>::ConstPtr FieldComparisonConstPtr;
      
      typedef typename pcl::traits::fieldList<PointT>::type FieldList;

      /** \brief Default constructor for the WorldModel.
       */
      WorldModel(double resolution = 0.1f) : 
         world (new PointCloud),
         octree (resolution),
         cpt_updates (0)
//         octree (new pcl::octree::OctreePointCloudChangeDetector<PointT> (resolution))
      {
//        octree(0.1f);
      }

      /** \brief Add a new point cloud in the world
       * \param[in] new_cloud the point cloud to add to the world
       */
      void addCloud(double originX, double originY, double originZ, double sizeX, double sizeY, double sizeZ, PointCloudPtr new_cloud);

      /** \brief Set the resolution of the 3D grid representing our world
       * \param[in] resolution the size of a 3D cell.
       */
      void setResolution(const double resolution);

      /** \brief Get the resolution of the 3D grid representing our world
       */
      double getResolution();

      /** \brief Returns the world as a point cloud.
       */
      PointCloudPtr getWorld();


      /** \brief Returns the world as a vector of cubes of size "size"
       *  \param[in] size the size of a 3D cube.
       *  \param[out] cubes a vector of point clouds representing each cube (in their original world coordinates). 
       *  \param[out] transforms a vector containing the xyz position of each cube in world coordinates.
       */
      void getWorldAsCubes(double size, std::vector<PointCloudPtr> &cubes, std::vector<Eigen::Vector3f> &transforms);
      
      
      void setSliceAsNan(double originX, double originY, double originZ, double maxX, double maxY, double maxZ);


      bool getExistingData(double previousOriginX, double previousOriginY, double previousOriginZ,
                           double offsetX, double offsetY, double offsetZ,
                           double volumeX, double volumeY, double volumeZ, pcl::PointCloud<PointT> &existing_slice);

    private:
      // cloud containing our world
      PointCloudPtr world;
      // octree used to detect new points
      pcl::octree::OctreePointCloudChangeDetector<PointT> octree;
      //counter for the number of time we add a cloud 
      int cpt_updates;

  };
}

#endif // PCL_WORLD_MODEL_H_
